﻿//-------------------------------------------------------------------
//	Copyright (c) 2010-2014  Zhirnov Andrey
//	This file is part of the "UX Standard Lib" project.
//	See copyright notice in "UX_Components.h".
//-------------------------------------------------------------------

#include "Ballistics.h"

namespace UX_STL
{
namespace UXPhysics
{
	using namespace Ballistics;


//--------------------------- TBullet -----------------------------//
			
	const double	TBullet::_s_dTimeStep	= 0.0001;
	const float		TBullet::_s_fPowerB		= 0.1f;

/*
=================================================
	конструктор
	расчитывается начальная скорость пули
	(TODO: разброс скоростей, зависимость от температуры)
=================================================
*/
	TBullet::TBullet(const TCartridge &sCartridge, const fvec3 &vDir, float fBarrelLength):
		_sCartridge(sCartridge), _vDirection(vDir), _fGravity(0.f), _fVelocity(0.f)
	{
		_fVelocity = sCartridge.vPower[0] / (fBarrelLength + _s_fPowerB) + sCartridge.vPower[1];
	}
	
/*
=================================================
	расчет полета пули
=================================================
*/
	void TBullet::Update(const TAtmosphere &sAtm, float fTimeDelta, fvec3 &vDelta)
	{
		static const float	c_fAirMolarMass	= AirMolarMass<float>();
		static const float	c_fWaterMM		= WaterMolarMass<float>();
		static const float	c_fGasConstant	= GasConst<float>();

		const float			f_T				= CelsiusToKelvin( sAtm.fT );
		const float			f_ss			= SpeedOfSoundInAir( f_T );
		const float			f_p_water		= WaterVaporPressure( sAtm.fAirHumidity, f_T );
		const float			f_p_air			= sAtm.fPressure - f_p_water;
		const float			f_dencity		= ( f_p_air * c_fAirMolarMass + f_p_water * c_fWaterMM ) /
											  ( f_T * c_fGasConstant );		// r = ( air(P*u) + water(P*u) ) / R*T
		double				d_time			= 0.0;
		double				d_dist			= 0.0;
		double				d_vel			= _fVelocity;
		double const		d_max_time		= fTimeDelta;


		while ( d_time < d_max_time and d_vel > 1.0 )
		{
			//		V^1.5 * k0, V > speed of sound
			// a = {
			//		k1/(V-SS*1.1) + k2, V <= speed of sound

			const double	a = ( d_vel > f_ss ) ?
								( d_vel * Sqrt( d_vel ) * _sCartridge.vBC[0] ) :
								( _sCartridge.vBC[1] / (d_vel - f_ss*1.1) + _sCartridge.vBC[2] );

			d_time	+= _s_dTimeStep;
			d_dist	+= d_vel * _s_dTimeStep - a * Square( _s_dTimeStep ) * 0.5;
			d_vel	-= a * _s_dTimeStep;
		}

		_fVelocity	 = (float)d_vel;
		vDelta		 = ( _vDirection.To<dvec3>() * d_dist + sAtm.vGravityDir.To<dvec3>() * ( 
						 double(_fGravity * fTimeDelta) +
						 double( sAtm.fGravity - _sCartridge.fAForce ) * Square( (double)fTimeDelta ) * 0.5 ) ).To<fvec3>();
		_fGravity	+= ( sAtm.fGravity - _sCartridge.fAForce ) * fTimeDelta;
	}
	
	


//----------------------- DefaultCartridge ------------------------//
	
	array< DefaultCartridge::TExtCartridge >	DefaultCartridge::_s_aCartridges;

/*
=================================================
	заполнить массив стандартными патронами
=================================================
*/
	void DefaultCartridge::Init()
	{
		TCartridge	s_cartridge;
		TAtmosphere	s_atm;

		DefAtmosphere( s_atm );
		s_atm.fT = 15.f;
		
		// 5.45х39 7Н6 из РПК-74
		{
			const float		a_vel[]		= { 960.f, 857.f, 760.f, 671.f, 588.f, 510.f,
											437.f, 372.f, 323.f, 298.f, 279.f };
			const float		a_times[]	= { 0.f, 0.11f, 0.23f, 0.37f, 0.53f, 0.72f,
											0.93f, 1.18f, 1.48f, 1.8f, 2.15f };
			
			Utils::CalcBC( s_cartridge, a_vel, a_times, 100.f, s_atm );
			Utils::CalcAscensionalForce( s_cartridge, 4.9f, 1.94f, s_atm );
			Utils::CalcPower( s_cartridge, fvec2( 735.f, 960.f ), fvec2( 0.2065f, 0.590f ) );	// АКС-74У и РПК-74

			s_cartridge.fBulletMass		= 3.42f / 1000.f;
			s_cartridge.fCaliber		= 5.45f / 1000.f;
			s_cartridge.fMass			= 10.5f / 1000.f;
			s_cartridge.fBulletLength	= 25.5f / 1000.f;
			s_cartridge.sInfo			= "5.45x39 7Н6 (ПС), с обыкновенной пулей";

			Add( S_545_39_PS, s_cartridge );
		}

		// 5.45x39 7Н10
		{
			s_cartridge.fBulletMass		= 3.60f / 1000.f;
			s_cartridge.fCaliber		= 5.45f / 1000.f;
			s_cartridge.fMass			= 10.7f / 1000.f;
			s_cartridge.fBulletLength	= 25.7f / 1000.f;
			s_cartridge.sInfo			= "5.45x39 7Н10 (ПП), с пулей повышенной пробиваемости";

			Add( S_545_39_PP, s_cartridge );
		}

		// 5.45x39 7Н22
		{
			s_cartridge.fBulletMass		= 3.70f / 1000.f;
			s_cartridge.fCaliber		= 5.45f / 1000.f;
			s_cartridge.fMass			= 10.7f / 1000.f;
			s_cartridge.fBulletLength	= 25.7f / 1000.f;
			s_cartridge.sInfo			= "5.45x39 7Н22 (БП), с бронебойной пулей";

			Add( S_545_39_BP, s_cartridge );
		}


		// 7.62x39 из РПК
		{
			const float		a_vel[]		= { 745.f, 650.f, 562.f, 482.f, 409.f, 347.f,
											311.f, 289.f, 271.f, 255.f, 240.f };
			const float		a_times[]	= { 0.f, 0.14f, 0.31f, 0.51f, 0.74f, 1.0f,
											1.29f, 1.62f, 1.98f, 2.36f, 2.76f };
			
			Utils::CalcBC( s_cartridge, a_vel, a_times, 100.f, s_atm );
			Utils::CalcAscensionalForce( s_cartridge, 5.2f, 1.98f, s_atm );
			Utils::CalcPower( s_cartridge, fvec2( 715.f, 745.f ), fvec2( 0.415f, 0.590f ) );	// АК и РПК

			s_cartridge.fBulletMass		= 7.90f / 1000.f;
			s_cartridge.fCaliber		= 7.62f / 1000.f;
			s_cartridge.fMass			= 16.2f / 1000.f;
			s_cartridge.fBulletLength	= 26.8f / 1000.f;
			s_cartridge.sInfo			= "7.62x39 57-Н-231 (ПС), с пулей со стальным сердечником";

			Add( S_762_39_PS, s_cartridge );
		}


		// 7.62x39 УС из АКМ с ПБС-1
		{
			const float		a_vel[]		= { 310.f, 288.f, 271.f, 258.f, 249.f };
			const float		a_times[]	= { 0.f, 0.34f, 0.69f, 1.06f, 1.45f };
			
			Utils::CalcBC( s_cartridge, a_vel, a_times, 100.f, s_atm );
			Utils::CalcAscensionalForce( s_cartridge, 2.58f, 1.45f, s_atm );
			Utils::CalcPower( s_cartridge, fvec2( 310.f, 0.f ), fvec2( 0.415f, 0.0f ) );	// АКМ и ...

			s_cartridge.fBulletMass		= 12.55f / 1000.f;
			s_cartridge.fCaliber		= 7.62f / 1000.f;
			s_cartridge.fMass			= 20.0f / 1000.f;
			s_cartridge.fBulletLength	= 33.6f / 1000.f;
			s_cartridge.sInfo			= "7.62x39 57-Н-231У (УС), с уменьшенной скоростью пули со стальным сердечником";

			Add( S_762_39_US, s_cartridge );
		}


		// 7.62x54 из ПК
		{
			const float		a_vel[]		= { 825.f, 746.f, 675.f, 608.f, 545.f, 486.f, 432.f, 384.f,
											343.f, 316.f, 299.f, 284.f, 270.f, 257.f, 245.f, 233.f };
			const float		a_times[]	= { 0.f, 0.13f, 0.27f, 0.43f, 0.6f, 0.79f, 1.01f, 1.26f, 1.53f,
											1.83f, 2.15f, 2.49f, 2.85f, 3.23f, 3.63f, 4.06f };
			
			Utils::CalcBC( s_cartridge, a_vel, a_times, 100.f, s_atm );
			Utils::CalcAscensionalForce( s_cartridge, 4.3f, 1.83f, s_atm );
			Utils::CalcPower( s_cartridge, fvec2( 855.f, 825.f ), fvec2( 0.722f, 0.658f ) );	// ПКТ и ПК

			s_cartridge.fBulletMass		= 9.60f / 1000.f;
			s_cartridge.fCaliber		= 7.62f / 1000.f;
			s_cartridge.fMass			= 21.8f / 1000.f;
			s_cartridge.fBulletLength	= 32.3f / 1000.f;
			s_cartridge.sInfo			= "7.62x54 57-Н-323С (ЛПС), с пулей со стальным сердечником";

			Add( S_762_54_LPS, s_cartridge );
		}

		
		// 12.7х108 Б-32 из ОСВ-96
		{
			const float		a_vel[]		= { 820.f, 780.f, 741.f, 703.f, 666.f, 631.f, 597.f,
											564.f, 532.f, 501.f, 471.f, 443.f, 416.f, 390.f,
											366.f, 345.f, 328.f, 314.f, 303.f, 294.f, 287.f };
			const float		a_times[]	= { 0.f, 0.13f, 0.26f, 0.4f, 0.55f, 0.7f, 0.86f, 1.03f,
											1.21f, 1.4f, 1.61f, 1.83f, 2.06f, 2.31f, 2.58f,
											2.86f, 3.16f, 3.47f, 3.79f, 4.12f, 4.47f };
			
			Utils::CalcBC( s_cartridge, a_vel, a_times, 100.f, s_atm );
			Utils::CalcAscensionalForce( s_cartridge, 4.9f, 1.94f, s_atm );
			Utils::CalcPower( s_cartridge, fvec2( 820.f, 0.f ), fvec2( 1.f, 0.f ) );	// ОСВ-96

			s_cartridge.fBulletMass		= 48.3f / 1000.f;
			s_cartridge.fCaliber		= 12.7f / 1000.f;
			s_cartridge.fMass			= 137.f / 1000.f;
			s_cartridge.sInfo			= "12.7х108 Б-32";
			s_cartridge.fBulletLength	= 64.6f / 1000.f;

			Add( S_127_108_B32, s_cartridge );
		}
	}
	
/*
=================================================
	возвращает стандартные глобальные параметры
=================================================
*/
	void DefaultCartridge::DefAtmosphere(TAtmosphere &sAtm)
	{
		sAtm.fAirHumidity	= 0.78f;
		sAtm.fGravity		= EarthGravity<float>();
		sAtm.fPressure		= AtmosphereToPa<float>();
		sAtm.fT				= 5.f;
		sAtm.vGravityDir	= fvec3( 0.f, 0.f, -1.f );
		sAtm.vWindDir		= fvec3( 1.f, 0.f, 0.f );
		sAtm.fWindVel		= 0.f;
	}

/*
=================================================
	добавить стандартный патрон
=================================================
*/
	bool DefaultCartridge::Add(ECartridge eType, const TCartridge &sCartridge)
	{
		TCartridge	tmp;

		if ( Get( eType, tmp ) )
			return false;

		_s_aCartridges.PushBack( TExtCartridge( sCartridge, eType ) );
		return true;
	}
	
/*
=================================================
	возвращает стандартный патрон
=================================================
*/
	bool DefaultCartridge::Get(ECartridge eType, TCartridge &sCartridge)
	{
		FOR( i, _s_aCartridges )
		{
			if ( eType == _s_aCartridges[i].eType ) {
				sCartridge = _s_aCartridges[i].sCartridge;
				return true;
			}
		}
		return false;
	}
	
	


//--------------------------- Utils -------------------------------//
/*
=================================================
	рассчитать коэффициенты для функции
	сопротивления воздуха
=================================================
*/
	void Utils::CalcBC(TCartridge &sCartridge, const uni_array<float> &aVelocities, const uni_array<float> &aFlyTimes,
						float fStep, const TAtmosphere &sAtm)
	{
		ASSERT( aVelocities.Count() == aFlyTimes.Count() );

		float			f_k0_sum		= 0.f;
		uint			u_k0_count		= 0;
		fvec2			v_k12;
		bool			b_k12			= false;
		bool			b_k0_after_k12	= false;
		const usize		u_count			= Min( aVelocities.Count(), aFlyTimes.Count() )-1;
		const float		f_ss			= SpeedOfSoundInAir( CelsiusToKelvin( sAtm.fT ) );

		ASSERT( u_count != MaxValue(u_count) );

		for (usize i = 0; i < u_count; ++i)
		{
			fvec2	v;	// a(V)
			float	dt =  aFlyTimes[i+1] - aFlyTimes[i];

			v.x = fStep / dt;									// V
			v.y = ( aVelocities[i] - aVelocities[i+1] ) / dt;	// a

			if ( v.x > f_ss )
			{
				f_k0_sum += v.y / ( v.x * Sqrt(v.x) );
				++u_k0_count;
			}
			else
			{
				v_k12 = v;
				b_k12 = true;
				break;
			}
		}


		// calc K0
		if ( u_k0_count != 0 )
			sCartridge.vBC[0] = f_k0_sum / (float)u_k0_count;
		else
			b_k0_after_k12 = true;


		// calc K1 & K2
		if ( !b_k12 )
		{
			if ( b_k0_after_k12 ) {
				WARNING( "not enough parameters" );
				return;
			}

			v_k12 = fvec2( f_ss, sCartridge.vBC[0] * f_ss * Sqrt(f_ss) );
		}

		const float	y0 = 0.f;
		const float	y1 = v_k12.y;
		const float	x0 = 0.f;
		const float	x1 = v_k12.x;
		const float	c  = -f_ss * 1.1f;

		sCartridge.vBC[1] = (y0 - y1) * (x0*x1 + c*x0 + c*x1 + c*c) / (x1 - x0);
		sCartridge.vBC[2] = y0 - ( sCartridge.vBC[1] / (x0 + c) );

		if ( b_k0_after_k12 )
		{
			const fvec2	v( f_ss, sCartridge.vBC[1] / (-f_ss*0.1f) + sCartridge.vBC[2] );
			sCartridge.vBC[0] = v.y / ( v.x * Sqrt(v.x) );
		}
	}
	
/*
=================================================
	рассчитать подъемную силу действующую на пулю
=================================================
*/
	void Utils::CalcAscensionalForce(TCartridge &sCartridge, float fHeightDelta, float fTimeDelta, const TAtmosphere &sAtm)
	{
		sCartridge.fAForce = sAtm.fGravity - 2.f * fHeightDelta / Square( fTimeDelta );
	}
	
/*
=================================================
	рассчитать зависимость начальной скорости
	пули от длинны ствола
=================================================
*/
	void Utils::CalcPower(TCartridge &sCartridge, const fvec2 &aV, const fvec2 &aBarrel)
	{
		static const float	fK = TBullet::_s_fPowerB;
		
		sCartridge.vPower[0] = - (aV[0] - aV[1]) * (aBarrel[0] * aBarrel[1] + fK * aBarrel[0] +
							  fK * aBarrel[1] + Square( fK )) / (aBarrel[0] - aBarrel[1]);
		sCartridge.vPower[1] = aV[1] - sCartridge.vPower[0] / (aBarrel[1] + fK);
	}
	
/*
=================================================
	моделировать полет пули до заданной дальности
=================================================
*/
	void Utils::Simulate(const TCartridge &sCartridge, float fBarrel, const TAtmosphere &sAtm,
						 float fDistance, float &fTime, float &fHeight, float &fVelocity)
	{
		static const float	c_fAirMolarMass	= AirMolarMass<float>();
		static const float	c_fWaterMM		= WaterMolarMass<float>();
		static const float	c_fGasConstant	= GasConst<float>();

		const float			f_T				= CelsiusToKelvin( sAtm.fT );
		const float			f_ss			= SpeedOfSoundInAir( f_T );
		const float			f_p_water		= WaterVaporPressure( sAtm.fAirHumidity, f_T );
		const float			f_p_air			= sAtm.fPressure - f_p_water;
		const float			f_dencity		= ( f_p_air * c_fAirMolarMass + f_p_water * c_fWaterMM ) /
											  ( f_T * c_fGasConstant );		// r = air(P*u) + water(P*u) / R*T
		double				d_time			= 0.0;
		double				d_dist			= 0.0;
		double				d_vel			= sCartridge.vPower[0] / (fBarrel + TBullet::_s_fPowerB) + sCartridge.vPower[1];
		double const		d_max_dist		= fDistance;


		while ( d_dist <= d_max_dist and d_vel > 1.0 )
		{
			const double	a = ( d_vel > f_ss ) ?
								( d_vel * Sqrt( d_vel ) * sCartridge.vBC[0] ) :
								( sCartridge.vBC[1] / (d_vel - f_ss*1.1) + sCartridge.vBC[2] );

			d_time	+= TBullet::_s_dTimeStep;
			d_dist	+= d_vel * TBullet::_s_dTimeStep - a * Square( TBullet::_s_dTimeStep ) * 0.5;
			d_vel	-= a * TBullet::_s_dTimeStep;
		}

		fTime		= (float)d_time;
		fVelocity	= (float)d_vel;
		fHeight		= ( sAtm.fGravity - sCartridge.fAForce ) * (float)Square( d_time ) * 0.5f;
	}
	
/*
=================================================
	установить прицел в 0 (по высоте) на
	заданной дистанции
=================================================
*/
	void Utils::ZeroToDist(const TCartridge &sCartridge, float fBarrel, const TAtmosphere &sAtm,
							float fDistance, fvec3 &vDirection)
	{
		float	f_time,
				f_vel,
				f_height;
		Simulate( sCartridge, fBarrel, sAtm, fDistance, f_time, f_height, f_vel );
		
		vDirection = ( vDirection - (f_height * sAtm.vGravityDir) / fDistance ).Normalized();
	}
	
/*
=================================================
	коррекция по высоте,
	дает поправку в несколько миллиметров
	(fSightingLineHeight - высота прицельной линии над стволом,
	 fZeroToDist - дистанция, на которой превышение траектории над линией прицеливания равно 0)
=================================================
*/
	float Utils::HeightCorrection(float fSightingLineHeight, float fZeroToDist, float fDistance)
	{
		return ( fSightingLineHeight * fDistance / fZeroToDist - fSightingLineHeight );
	}

//-------------------------------------------------------------------

}	// UXPhysics
}	// UX_STL